/*
 * cilindro.c
 *
 *  Created on: 17/02/2013
 *      Author: alex
 */

#include "../headers/cilindro.h"

// Función que calcula la distancia de la intersección con un cilindro
long double IntCilindro(long double Xe, long double Ye, long double Ze,
                        long double Xd, long double Yd, long double Zd,
                        long double X0, long double Y0, long double Z0,
                        long double Xq, long double Yq, long double Zq,
                        long double A, long double B, long double C,
                        long double Back, long double Front, long double R2,
                        objeto_t *objeto)
{
  long double a, b, c, t1, t2, delta;
  long double D,E,F,G,H,I;
  long double Xi1, Yi1, Zi1, Xi2, Yi2, Zi2, d;

  	D = ((Xq * Xq * Xd) + (A * Yd) + (B * Zd) - Xd);
  	E = ((Yq * Yq * Yd) + (A * Xd) + (C * Zd) - Yd);
  	F = ((Zq * Zq * Zd) + (B * Xd) + (C * Yd) - Zd);
  	G = (Xe - X0);
  	H = (Ye - Y0);
  	I = (Ze - Z0);

  	a = ((D * D) + (E * E) + (F * F));

  	b = 2 * ((X0 * D) + (Y0 * E) + (Z0 * F) - (Xe * D) - (Ye * E) - (Ze * F)
  			 + (G * ((Xq * Xq * D) + (E * A) + (F * B)))
  			 + (H * ((D * A) + (Yq * Yq * E) + (F * C)))
  			 + (I * ((D * B) + (E * C) + (Zq * Zq * F))));

  	c = (X0 * X0) + (Y0 * Y0) + (Z0 * Z0) + (Xe * Xe) + (Ye * Ye) + (Ze * Ze);
  	c += (G * G * (pow(Xq,4) + (A * A) + (B * B)));
  	c += (H * H * (pow(Yq,4) + (A * A) + (C * C)));
  	c += (I * I * (pow(Zq,4) + (B * B) + (C * C)));
  	c += 2 * (G * ((X0 * Xq * Xq) - (Xe * Xq * Xq) + (Y0 * A) - (Ye * A) + (Z0 * B) - (Ze * B)));
  	c += 2 * (H * ((X0 * A) - (Xe * A) + (Y0 * Yq * Yq) - (Ye * Yq * Yq) + (Z0 * C) - (Ze * C)));
  	c += 2 * (I * ((X0 * B) - (Xe * B) + (Y0 * C) - (Ye * C) + (Z0 * Zq * Zq) - (Ze * Zq * Zq)));
  	c += (2 * G * H * ((Xq * Xq * A) + (Yq * Yq * A) + (B * C)));
  	c += (2 * G * I * ((Xq * Xq * B) + (A * C) + (Zq * Zq * B)));
  	c += (2 * H * I * ((A * B) + (Yq * Yq * C) + (Zq * Zq * C)));
  	c += -2 * ((X0 * Xe) + (Y0 * Ye) + (Z0 * Ze));
  	c -= R2;

  	delta = (pow(b, 2) - 4.0 * a * c);

  	if (delta < EPSILON)
  	{
  		//No hay solución, es decir, no hay intersección con el rayo
  		return -1.0;
  	}
  	else
  	{
  		t1 = ((-b - sqrt(delta)) / (2.0 * a));
  		t2 = ((-b + sqrt(delta)) / (2.0 * a));

  		//Calcula el punto de intersección con el objeto
  		Xi1 = (Xe + t1 * Xd);
  		Yi1 = (Ye + t1 * Yd);
  		Zi1 = (Ze + t1 * Zd);

  		Xi2 = (Xe + t2 * Xd);
  		Yi2 = (Ye + t2 * Yd);
  		Zi2 = (Ze + t2 * Zd);

  		//Compara las t con los bordes del cilindro
  		if (Back < Front)
  		{
  			if (t1 > EPSILON)
  			{
  				d = (((Xi1 - X0) * Xq) + ((Yi1 - Y0) * Yq) + ((Zi1 - Z0) * Zq));
  				if ((d < Back) || (d > Front))
  					t1 = 0.0;
  			}

  			if (t2 > EPSILON)
  			{
  				d = (((Xi2 - X0) * Xq) + ((Yi2 - Y0) * Yq) + ((Zi2 - Z0) * Zq));
  				if ((d < Back) || (d > Front))
  					t2 = 0.0;
  			}
  		}

  		if ((t1 < EPSILON) && (t2 < EPSILON)) //Está detrás del ojo
  			return -1;
  		if ((t1 < EPSILON) && (t2 > EPSILON)) // t2 delante del ojo
  		{
  			//Corta el objeto y verifica que el punto sea válido
			if (Cortar(objeto,Xi2,Yi2,Zi2) == 1)
  				return t2;
  			else
  				return -1;
  		}
  		if ((t1 > EPSILON) && (t2 < EPSILON)) // t1 delante del ojo
  		{
  			//Corta el objeto y verifica que el punto sea válido
			if (Cortar(objeto,Xi1,Yi1,Zi1) == 1)
  				return t1;
  			else
  				return -1;
  		}
  		if ((t1 > EPSILON) && (t2 > EPSILON)) // Ambos delante del ojo
  		{
  			// Devuelve el más cercano al ojo
  			if (t1 < t2)
  			{
  				if (Cortar(objeto,Xi1,Yi1,Zi1) == 1)
  					return t1;
  				else if (Cortar(objeto,Xi2,Yi2,Zi2) == 1)
  					return t2;
  				else
  					return -1;
  			}
  			else
  			{
  				if (Cortar(objeto,Xi2,Yi2,Zi2) == 1)
  					return t2;
  				else if (Cortar(objeto,Xi1,Yi1,Zi1) == 1)
  					return t1;
  				else
  					return -1;
  			}
  		}
  	}
}

// Función que calcula el vector normal al punto de intersección con el cilindro
long double* NormalCilindro(long double Xi, long double Yi, long double Zi,
						    long double X0, long double Y0, long double Z0,
						    long double Xq, long double Yq, long double Zq,
						 	long double R)
{
  long double* Normal;
  long double Xm, Ym, Zm, temp;

  	temp = (((Xi - X0) * Xq) + ((Yi - Y0) * Yq) + ((Zi - Z0) * Zq));
  	Xm = (X0 + Xq * temp);
	Ym = (Y0 + Yq * temp);
	Zm = (Z0 + Zq * temp);

  	Normal = (long double*)malloc(3 * sizeof(long double));
  	Normal[0] = Xi - Xm;
  	Normal[1] = Yi - Ym;
  	Normal[2] = Zi - Zm;
  	Normal[0] = Normal[0] / R;
  	Normal[1] = Normal[1] / R;
  	Normal[2] = Normal[2] / R;

  	return Normal;
}
